#include "vehicle_ll_mpc.h"
#include "poly2d.h"
#include <cmath>
namespace auto_ros
{
namespace control
{

VehicleLLMpc::VehicleLLMpc(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
	: VehicleLMpcBase(dyn_mpc_kf_config_path, vehicle_lmpc_yaml_name, rate)
{
	init_mpc();
	dynpar_map_ = {{"speed", 1.0}};
	ref_state_slover_ptr_->add_slack_states({0, 4});
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(1) + par(vehicle_dyn_ptr_->parm_["lf"]) * ref_state_slover_ptr_->x_(2)) / dynpar(dynpar_map_["speed"]) - ref_state_slover_ptr_->u_(0),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(1) - par(vehicle_dyn_ptr_->parm_["lr"]) * ref_state_slover_ptr_->x_(2)) / dynpar(dynpar_map_["speed"]),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->init_solver_ptr();

} //
void VehicleLLMpc::init_mpc()
{
	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_, "tustin");
	int poly_num = mpc_yaml_["poly_num"].as<int>();
	double alpha_f_max = mpc_yaml_["alpha_f_max"].as<double>() / 180 * M_PI;
	double alpha_r_max = mpc_yaml_["alpha_r_max"].as<double>() / 180 * M_PI;

	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 2 /*max_xu_constraint*/, 0, 1 /*max_delta_u*/, 0, 0,
											 poly_num + poly_num / 2 + 1 /*max_xu_slack_ineq = 0*/, 0, 0, 0 /*max_delta_u_ae*/, 0, 0, 0);
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1.0}, 0, -mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14,
									mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {1}, {1.0}, 1, -mpc_yaml_["de_acc_x_max"].as<double>(),
									mpc_yaml_["acc_f_x_max"].as<double>());
	/*
	int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
					   const std::vector<double> u_index, const std::vector<double> u_factor,
					   int constraint_index, double min, double max)*/
	update_friction_circle(1.0);
	//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_rate"].as<double>() / rate_);
}
bool VehicleLLMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
						const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
						double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
						double tire_r_alpha_w)
{
	vehicle_dyn_ptr_->para_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLLMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLLMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool VehicleLLMpc::step(const Eigen::VectorXd &x0,
						const Eigen::VectorXd &u_fb,
						const PlanningRefPoint &ref_point,
						double speed)
{
	double speed_threshold = vehicle_dyn_ptr_->dyn_yaml_["speed_threshold"].as<double>();
	double speed_limited;
	if (std::abs(speed) < speed_threshold)
	{
		if (speed >= 0)
			speed_limited = speed_threshold;
		else
			speed_limited = -speed_threshold;
	}
	else
	{
		speed_limited = speed;
	}
	Eigen::VectorXd temp_state_esti(6);
	temp_state_esti << speed, x0(1), x0(2), 0, 0, 0;

	Eigen::VectorXd y(kf_lateral_ptr_->con_ptr_->out_dim());
	y << speed, x0(1), x0(2);
	Eigen::VectorXd control_last(kf_lateral_ptr_->con_ptr_->control_dim());
	control_last = u_fb;
	kf_lateral_ptr_->step(control_last, y, speed);

	Eigen::VectorXd state_esti = kf_lateral_ptr_->state_esti();
	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["longti_speed_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["lateral_speed_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["yaw_rate_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(4, 4) = mpc_yaml_["speed_gama_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	r_temp(0, 0) = mpc_yaml_["steering_f_factor_max"].as<double>();
	r_temp(1, 1) = mpc_yaml_["acc_f_factor"].as<double>();
	Eigen::VectorXd x_ref(dis_dyn_ptr_->state_dim());
	Eigen::VectorXd u_ref(dis_dyn_ptr_->control_dim());
	Eigen::VectorXd e_y_ref(dis_dyn_ptr_->out_dim());
	e_y_ref << 0.0, 0.0;

	vehicle_dyn_ptr_->para_update_matrix(speed_limited);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)*/
	Eigen::VectorXd distur_ref(dis_dyn_ptr_->distur_dim());
	if (std::abs(speed) < speed_threshold)
	{
		distur_ref << ref_point.kappa_, ref_point.a_, 0, 0, 0; //state_esti(3), , state_esti(5);
	}
	else
	{
		distur_ref << ref_point.kappa_, ref_point.a_, state_esti(3), state_esti(4), state_esti(5); //state_esti(3), state_esti(4), state_esti(5);
	}
	//vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	//dis_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	dynpar_map_["speed"] = speed_limited;
	Eigen::VectorXd slack_vec(vehicle_dyn_ptr_->state_dim());
	ref_state_slover_ptr_->calc_steady_state(distur_ref, x_ref, u_ref, slack_vec);
	x_ref_ = x_ref;
	u_ref_ = u_ref;
	update_friction_circle(speed_limited);
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/

	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q_temp, r_temp, {}, {/**/},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLLYawMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
		reset_kf(temp_state_esti);
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		reset_kf(temp_state_esti);
		std::cout << "\033[31m Error:VehicleLLYawMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
void VehicleLLMpc::update_friction_circle(double speed)
{

	int poly_num = mpc_yaml_["poly_num"].as<int>();
	double alpha_f_max = mpc_yaml_["alpha_f_max"].as<double>() / 180 * M_PI;
	double alpha_r_max = mpc_yaml_["alpha_r_max"].as<double>() / 180 * M_PI;
	std::vector<double> xu_slack_ineq_vec;
	//Mapping front friction circles to state and control spaces
	for (int i = 0; i < poly_num; i++)
	{
		xu_slack_ineq_vec.push_back(mpc_yaml_["tire_f_friction_circle_w"].as<double>());
		double temp_f_acc_x_max = 0;
		if (std::cos(2 * M_PI / poly_num * i) > 0 || std::cos(2 * M_PI / poly_num * (i + 1)) > 0)
		{
			temp_f_acc_x_max = mpc_yaml_["acc_f_x_max"].as<double>();
		}
		else
		{
			temp_f_acc_x_max = mpc_yaml_["de_acc_x_max"].as<double>();
		}

		Point temp_point(temp_f_acc_x_max * std::cos(2 * M_PI / poly_num * i), alpha_f_max * std::sin(2 * M_PI / poly_num * i));
		Point temp_point1(temp_f_acc_x_max * std::cos(2 * M_PI / poly_num * (i + 1)), alpha_f_max * std::sin(2 * M_PI / poly_num * (i + 1)));
		LinearIneq temp_ineq(temp_point, temp_point1);
		//int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
		//				  const std::vector<double> u_index, const std::vector<double> u_factor,
		//				  int constraint_index, double min, double max);
		double coff_x = temp_ineq.coff_x();
		double coff_y = temp_ineq.coff_y();
		double coff_const = temp_ineq.coff_const();
		/*std::cout << "front tire constraints " << i << ":  "
				  << "coff_x:" << temp_ineq.coff_x() << "coff_y:"
				  << temp_ineq.coff_y() << "coff_const:" << temp_ineq.coff_const()
				  << " is_greater_than_zero:" << temp_ineq.is_greater_than_zero() << std::endl;*/
		if (temp_ineq.is_greater_than_zero() == true)
		{
			vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2}, {coff_y / speed, coff_y * vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / speed},
												{0, 1}, {-coff_y, coff_x}, i,
												-coff_const,
												OsqpSolver::INFTY);
		}
		else
		{
			vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2}, {coff_y / speed, coff_y * vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / speed},
												{0, 1}, {-coff_y, coff_x}, i,
												-OsqpSolver::INFTY,
												-coff_const);
		}
	}
	//Mapping rear friction circles to state and control spaces
	for (int i = 0; i < poly_num / 2; i++)
	{
		xu_slack_ineq_vec.push_back(mpc_yaml_["tire_r_friction_circle_w"].as<double>());
		double temp_r_acc_x_max = mpc_yaml_["de_acc_x_max"].as<double>();
		Point temp_point(temp_r_acc_x_max * std::cos(2 * M_PI / poly_num * i + 0.5 * M_PI),
						 alpha_r_max * std::sin(2 * M_PI / poly_num * i + 0.5 * M_PI));
		Point temp_point1(temp_r_acc_x_max * std::cos(2 * M_PI / poly_num * (i + 1) + 0.5 * M_PI),
						  alpha_r_max * std::sin(2 * M_PI / poly_num * (i + 1) + 0.5 * M_PI));
		LinearIneq temp_ineq(temp_point, temp_point1);
		//int set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
		//				  const std::vector<double> u_index, const std::vector<double> u_factor,
		//				  int constraint_index, double min, double max);
		double coff_x = temp_ineq.coff_x();
		double coff_y = temp_ineq.coff_y();
		double coff_const = temp_ineq.coff_const();
		/*std::cout << "front tire constraints " << i << ":  "
				  << "coff_x:" << temp_ineq.coff_x() << "coff_y:"
				  << temp_ineq.coff_y() << "coff_const:" << temp_ineq.coff_const()
				  << " is_greater_than_zero:" << temp_ineq.is_greater_than_zero() << std::endl;*/
		if (temp_ineq.is_greater_than_zero() == true)
		{
			vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2},
												{coff_y / speed, -coff_y * vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / speed},
												{1}, {coff_x}, i + poly_num,
												-coff_const, OsqpSolver::INFTY);
		}
		else
		{
			vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2}, {coff_y / speed, -coff_y * vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / speed},
												{1}, {coff_x}, i + poly_num,
												-OsqpSolver::INFTY,
												-coff_const);
		}
	}
	vehicle_mpc_ptr_->set_xu_slack_ineq({1, 2}, {1.0 / speed, -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / speed},
										{}, {}, poly_num + poly_num / 2,
										-alpha_r_max,
										alpha_r_max);
	xu_slack_ineq_vec.push_back(mpc_yaml_["tire_r_friction_circle_w"].as<double>());
	//End mapping rear friction circles to state and control spaces
	vehicle_mpc_ptr_->set_xu_slack_ineq_w(xu_slack_ineq_vec);
}
bool VehicleLLMpc::reset_kf()
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(),
							  kf_lateral_ptr_->con_ptr_->state_dim());
	Eigen::VectorXd state_esti =
		Eigen::VectorXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
bool VehicleLLMpc::reset_kf(const Eigen::VectorXd &state_esti)
{
	Eigen::MatrixXd state_cov_esti =
		Eigen::MatrixXd::Zero(kf_lateral_ptr_->con_ptr_->state_dim(), kf_lateral_ptr_->con_ptr_->state_dim());
	return kf_lateral_ptr_->reset(state_esti, state_cov_esti);
}
} // namespace control
} // namespace auto_ros
